package Simulator;

import javax.media.j3d.Transform3D;
import javax.media.j3d.TransformGroup;
import javax.vecmath.Vector3f;

import com.sun.j3d.utils.geometry.Cylinder;
import com.sun.j3d.utils.geometry.Sphere;

public class Robot {
	
	TransformGroup base = new TransformGroup();
	Transform3D basem = new Transform3D();
//	TransformGroup joint1 = new TransformGroup();
//	Transform3D joint1m = new Transform3D();
	TransformGroup joint2 = new TransformGroup();
	Transform3D joint2m = new Transform3D();
	TransformGroup link1 = new TransformGroup();
	Transform3D link1m = new Transform3D();
	TransformGroup joint3 = new TransformGroup();
	Transform3D joint3m = new Transform3D();
	TransformGroup link2 = new TransformGroup();
	Transform3D link2m = new Transform3D();
//	TransformGroup joint4 = new TransformGroup();
//	Transform3D joint4m = new Transform3D();
	TransformGroup joint5 = new TransformGroup();
	Transform3D joint5m = new Transform3D();
	TransformGroup joint6 = new TransformGroup();
	Transform3D joint6m = new Transform3D();
	TransformGroup endPoint = new TransformGroup();
	Transform3D endPointm = new Transform3D();
	Transform3D someRotation = new Transform3D();
	
	float link1Length = .55f;
	float link1Offset = (link1Length - .5f)/2;
	float link2Length = 0.55f;
	float link2Offset = (link2Length - .5f)/2;
	float baseLength = 0.55f;
	float baseOffset = (baseLength - .5f)/2;
	
	int baseAngle = 0;
	int joint2Angle = 0;
	int joint3Angle = -0;
	int joint5Angle = 0;
	int joint6Angle = 0;
	
	public void newRobot()
	{
		//create endLeft
		Cylinder endLeftx = new Cylinder(.020f, .15f);
		TransformGroup endLeft = new TransformGroup();
		Transform3D endLeftm = new Transform3D();
		endLeft.addChild(endLeftx);
		endLeftm.setTranslation(new Vector3f(0.075f,0.05f,.0f));
		someRotation = new Transform3D();
		someRotation.rotZ(-90 * (Math.PI/180));
		endLeftm.mul(someRotation);
		endLeft.setTransform(endLeftm);
		
		//create endright
		Cylinder endRightx = new Cylinder(.020f, .15f);
		TransformGroup endRight = new TransformGroup();
		Transform3D endRightm = new Transform3D();
		endRight.addChild(endRightx);
		endRightm.setTranslation(new Vector3f(0.075f,-0.05f,0.0f));
		someRotation = new Transform3D();
		someRotation.rotZ(-90 * (Math.PI/180));
		endRightm.mul(someRotation);
		endRight.setTransform(endRightm);
		
		//create endpoint
		Cylinder endPointx = new Cylinder(.020f, .1f);
		someRotation = new Transform3D();
		someRotation.rotZ(90 * (Math.PI/180));
		endPointm.mul(someRotation);
		endPoint.addChild(endPointx);
		endPoint.addChild(endRight);
		endPoint.addChild(endLeft);
		endPoint.setTransform(endPointm);
		
		//create joint6 sphere
		Sphere joint6x = new Sphere(.02f);
		joint6.addChild(joint6x);
		joint6m.setTranslation(new Vector3f(-.0f,0.27f,0.0f));
		someRotation = new Transform3D();
		someRotation.rotX(joint6Angle * (Math.PI/180));		
		joint6m.mul(someRotation);
		joint6.setTransform(joint6m);
		joint6.addChild(endPoint);
		
		//create joint5 sphere
		Sphere joint5x = new Sphere(.02f);
		joint5.addChild(joint5x);
		joint5m.setTranslation(new Vector3f(-.0f,0.0f + link2Offset,0.0f));
		someRotation = new Transform3D();
		someRotation.rotY(joint5Angle * (Math.PI/180));		
		joint5m.mul(someRotation);
		joint5.setTransform(joint5m);
		joint5.addChild(joint6);
		
		//create link2
		Cylinder link2x = new Cylinder(.07f, link2Length);
		link2.addChild(link2x);
		link2m.setTranslation(new Vector3f(-.05f,0.2f  + link2Offset,0.0f));
		someRotation = new Transform3D();
		//someRotation.rotX(Math.PI *.5);
		link2m.mul(someRotation);
		link2.setTransform(link2m);
		link2.addChild(joint5);
		
		//create joint3 sphere
		Sphere joint3x = new Sphere(.03f);
		joint3.addChild(joint3x);
		joint3m.setTranslation(new Vector3f(-.075f,.20f + link1Offset,0.0f));
		someRotation = new Transform3D();
		someRotation.rotX(joint3Angle * (Math.PI/180));		
		joint3m.mul(someRotation);
		joint3.setTransform(joint3m);
		joint3.addChild(link2);
		
		//create link1
		Cylinder link1x = new Cylinder(.07f, link1Length);
		link1.addChild(link1x);
		someRotation = new Transform3D();
		someRotation.rotZ(Math.PI *.5);
		link1m.mul(someRotation);
		link1m.setTranslation(new Vector3f(-0.20f - link1Offset,0.05f,-0.0f));
		link1.setTransform(link1m);
		link1.addChild(joint3);
		
		//sphere for joint 2
		TransformGroup joint2s = new TransformGroup();
		Transform3D joint2sm = new Transform3D();
		Sphere joint2sx = new Sphere(.03f);
		joint2s.addChild(joint2sx);
		joint2sm.setTranslation(new Vector3f(.02f,.09f,0.0f));
		someRotation = new Transform3D();
		someRotation.rotY(joint2Angle * (Math.PI/180));
		joint2sm.mul(someRotation);
		joint2s.setTransform(joint2sm);
		joint2s.addChild(link1);
	
		//create joint 2
		Cylinder joint2x = new Cylinder(0.05f,.2f);
		joint2.addChild(joint2x);
		joint2m.setTranslation(new Vector3f(-0.01f,0.30f+baseOffset,0.0f));
		//Transform3D someRotation = new Transform3D();
		someRotation.rotZ(Math.PI * -.5);
		joint2m.mul(someRotation);
		joint2.setTransform(joint2m);
		joint2.addChild(joint2s);
		
		
		//create base
		Cylinder basex = new Cylinder(0.07f,baseLength);
		someRotation = new Transform3D();
		someRotation.rotY(baseAngle * (Math.PI/180));
		basem.mul(someRotation);
		basem.setTranslation(new Vector3f(0.0f,0.0f,0.0f));
		base.setTransform(basem);
		base.addChild(basex);
		base.addChild(joint2);
	}
	
	public TransformGroup getBase()
	{
		return this.base;
	}
	
	public void setProperties()
	{
		this.link1Length = .95f;
		this.link2Length = 0.55f;
		this.baseLength = 0.55f;
		this.link1Offset = (link1Length - .5f)/2;
		this.link2Offset = (link2Length - .5f)/2;
		this.baseOffset = (baseLength - .5f)/2;
		
	}
	
}

